import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile
from std_msgs.msg import UInt8
from agv_srvs.srv import ControlVoice
import time

class VoiceControlServer(Node):
    def __init__(self):
        super().__init__('voice_control_server')
        
        # 初始化服务
        self.srv = self.create_service(
            ControlVoice, 
            '/agv_control_voice', 
            self.control_callback
        )
        self.num = 0
        self.last_sensor_time = None
        
        # 初始化发布者 (20Hz)
        self._diginput_publishers = {}  # 重命名
        self.init_publishers()
        
        # 初始化订阅者 (20Hz)
        self._digoutput_subscribers = {}  # 重命名
        self.init_subscribers()
        
        # 定时器控制发布频率 (20Hz = 0.05秒周期)
        self.timer = self.create_timer(0.05, self.timer_callback)
        
        self.get_logger().info("Voice Control Server Ready...")

    def init_publishers(self):
        """初始化所有数字输入发布者"""
        devices = [10, 11]
        for device in devices:
            self._diginput_publishers[f'device{device}'] = []  # 使用新名称
            for channel in range(1, 9):
                topic_name = f"/can1/device{device}/diginput8_{channel}"
                publisher = self.create_publisher(
                    UInt8, 
                    topic_name, 
                    QoSProfile(depth=10)
                )
                self._diginput_publishers[f'device{device}'].append(publisher)

    def init_subscribers(self):
        """初始化所有数字输出订阅者"""
        devices = [10, 11]
        for device in devices:
            self._digoutput_subscribers[f'device{device}'] = []  # 使用新名称
            for channel in range(1, 9):
                topic_name = f"/can1/device{device}/digoutput8_{channel}"
                subscriber = self.create_subscription(
                    UInt8,
                    topic_name,
                    lambda msg, d=device, c=channel: self.output_callback(msg, d, c),
                    QoSProfile(depth=10)
                )
                self._digoutput_subscribers[f'device{device}'].append(subscriber)

    # 其他方法保持不变（只需替换内部引用的变量名）

    def timer_callback(self):
        """定时发布数字输入数据（示例数据）"""
        self.num = self.num + 1;
        if self.num > 255:
            self.num = 0;

        for device in [10, 11]:
            value = self.num
            for idx, publisher in enumerate(self._diginput_publishers[f'device{device}'], start=1):
                msg = UInt8()
                msg.data = value  # 示例数据，需替换为实际采集值
                publisher.publish(msg)
                self.get_logger().debug(
                    f"Published to Device {device} Channel {idx}: {msg.data}"
                )
                value = value + 1
                if value > 255:
                    value = 0

    def control_callback(self, request, response):
        # 打印接收到的请求
        self.get_logger().info(
            f"Received request: audio={request.audio}, volume={request.volume}"
        )

        # 模拟处理逻辑（可根据需求修改）
        if request.audio >= 0 and request.volume >= 0:
            response.success = True
            self.get_logger().info("Voice control succeeded!")
        else:
            response.success = False
            self.get_logger().warn("Voice control failed: invalid parameters!")

        return response

    def output_callback(self, msg, device, channel):
        """处理订阅到的数字输出数据"""
        self.get_logger().debug(
            f"Device {device} Channel {channel} Output: {msg.data}"
        )

def main():
    rclpy.init()
    server = VoiceControlServer()
    try:
        rclpy.spin(server)
    except KeyboardInterrupt:
        server.get_logger().info("Server shutdown requested")
    finally:
        server.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()